
#include "linmin.h"
#include "polyfit.h"

#include <gsl/gsl_blas.h>

static const double mu = 0.001;
static const double chi = 1e-2;

typedef struct
{
  double alphal, alphat, alphau;
  double psil, psit, psiu;
  double dpsil, dpsit, dpsiu;
  double phil, phit, phiu;
  double dphil, dphit, dphiu;
  gsl_vector *xt;
  gsl_vector *gt;
  gsl_vector *gu;
  int alphau_evaluated;
  double gamma;
}
morethuente_state_t;

static double trialstep(double alphat,
                        double alphal,
                        double alphau,
                        double fl,
                        double dfl,
                        double ft,
                        double dft,
                        double fu,
                        double dfu,
                        const lineminimizer_setup *setup,
                        morethuente_state_t *state);

static void bracket(double *alphal, 
                    double alphat, 
                    double *alphau, 
                    double fl, 
                    double ft, 
                    double dft, 
                    double *phil, 
                    double phit, 
                    double *phiu, 
                    double *dphil, 
                    double dphit, 
                    double *dphiu);

static inline double psi(double phi, double alpha, double phi0, double dphi0);

static int morethuente_alloc(void *vstate, size_t n)
{
  morethuente_state_t *state = (morethuente_state_t *)vstate;
  
  state->xt = gsl_vector_alloc(n);
  state->gt = gsl_vector_alloc(n);
  state->gu = gsl_vector_alloc(n);
  
  state->gamma = 1.0;
  
  return GSL_SUCCESS;
}

static void morethuente_free(void *vstate)
{
  morethuente_state_t *state = (morethuente_state_t *)vstate;
  
  gsl_vector_free(state->gu);
  gsl_vector_free(state->gt);
  gsl_vector_free(state->xt);
}

void lineminimizer_morethuente_set_gamma(lineminimizer *lm, double gamma)
{
  ((morethuente_state_t *)lm->state)->gamma = gamma;
}

int morethuente_minimize(void *vstate,
                         const lineminimizer_setup *setup,
                         double alpha0,
                         double f,
                         double *alpha,
                         gsl_vector *x_plus,
                         double *f_plus,
                         gsl_vector *g_plus)
{
  double alphat_plus;
  double phi0, dphi0;
  int i = 0;
  int terminated = 0;
  double gNorm2;
  int suff_desc;
  int stage = 1;
  
  morethuente_state_t *state = (morethuente_state_t *)vstate;
  
  state->alphal = 0.0;
  state->alphau = 1e3;
  state->alphat = alpha0;
	
  state->alphau_evaluated = 0;
  
  phi0 = f;
  gsl_blas_ddot(setup->g, setup->d, &dphi0);
  
  state->phil = f;
  state->dphil = dphi0;
  
  while(1)
  {
    gsl_blas_dcopy(setup->x, state->xt);
    gsl_blas_daxpy(state->alphat, setup->d, state->xt);
    
    GSL_MULTIMIN_FN_EVAL_F_DF(setup->fdf, state->xt, &state->phit, state->gt);
    gsl_blas_ddot(state->gt, setup->d, &state->dphit);
    
    if(stage == 1)
    {
      state->psil = psi(state->phil, state->alphal, phi0, dphi0);
      state->psit = psi(state->phit, state->alphat, phi0, dphi0);
      state->psiu = psi(state->phiu, state->alphau, phi0, dphi0);
      
      state->dpsil = state->dphil - mu * dphi0;
      state->dpsit = state->dphit - mu * dphi0;
      state->dpsiu = state->dphiu - mu * dphi0;
    }
    
    if(i == 50 || fabs(state->alphal - state->alphau) < 10.0 * GSL_DBL_EPSILON)
    {
      terminated = 1;
      break;
    }
    
    /* test the sufficient descent condition */
    if(state->dphit <= 0.0 || state->gamma == 0.0)
      suff_desc = 1;
    else
    {
      gsl_blas_ddot(state->gt, state->gt, &gNorm2);
      if(-gNorm2 + state->gamma * state->dphit < -chi * gNorm2)
        suff_desc = 1;
      else
        suff_desc = 0;
    }
    suff_desc = 1;
    
    /* test the strong Wolfe conditions */
    if(state->phit <= phi0 + mu * state->alphat * dphi0 && 
       fabs(state->dphit) <= setup->eta * fabs(dphi0) && suff_desc)
      break;
    
    /* Step 1.:trial value selection */
    if(stage == 1)
      alphat_plus = trialstep(state->alphat, state->alphal, state->alphau, 
                              state->psil, state->dpsil, state->psit, state->dpsit, 
                              state->psiu, state->dpsiu, 
                              setup, state);
    else
      alphat_plus = trialstep(state->alphat, state->alphal, state->alphau, 
                              state->phil, state->dphil, state->phit, state->dphit, 
                              state->phiu, state->dphiu, 
                              setup, state);
    
    if(state->psit <= 0.0 && state->dphit > 0.0)
      stage = 2;
    
    /* Step 2.:bracketing */
    if(stage == 1)
      bracket(&state->alphal, state->alphat, &state->alphau, 
              state->psil, state->psit, state->dpsit, 
              &state->phil, state->phit, &state->phiu, 
              &state->dphil, state->dphit, &state->dphiu);
    else
      bracket(&state->alphal, state->alphat, &state->alphau, 
              state->phil, state->phit, state->dphit, 
              &state->phil, state->phit, &state->phiu, 
              &state->dphil, state->dphit, &state->dphiu);
    
    state->alphat = alphat_plus;
    
    i++;
  }
  
  *alpha = state->alphat;
  gsl_blas_dcopy(state->xt, x_plus);
  *f_plus = state->phit;
  gsl_blas_dcopy(state->gt, g_plus);
  
  if(terminated)
    return GSL_EDOM;
  else
    return GSL_SUCCESS;
}

static double trialstep(double alphat,
                        double alphal,
                        double alphau,
                        double fl,
                        double dfl,
                        double ft,
                        double dft,
                        double fu,
                        double dfu,
                        const lineminimizer_setup *setup,
                        morethuente_state_t *state)
{
  double alphac, alphaq, alphas, alphae;
  double alphat_plus;
  int status1, status2;
  
  if(ft > fl)
  {
    status1 = cubic_minimizer(alphal, fl, dfl, alphat, ft, dft, 0, &alphac);
    status2 = quad_minimizer1(alphal, fl, dfl, alphat, ft, &alphaq);
  
    if(status1 == GSL_SUCCESS && status2 == GSL_SUCCESS)
    {
      if(fabs(alphac - alphal) < fabs(alphaq - alphal))
        alphat_plus = alphac;
      else
        alphat_plus = 0.5 * (alphaq + alphac);
    }
    else
      goto bisection1;
  }
  else if(dft * dfl < 0.0)
  {
    status1 = cubic_minimizer(alphal, fl, dfl, alphat, ft, dft, 0, &alphac);
    status2 = quad_minimizer2(alphal, dfl, alphat, dft, &alphas);
  
    if(status1 == GSL_SUCCESS && status2 == GSL_SUCCESS)
    {
      if(fabs(alphac - alphat) >= fabs(alphas - alphat))
        alphat_plus = alphac;
      else
        alphat_plus = alphas;
    }
    else
      goto bisection1;
  }
  else if(fabs(dft) <= fabs(dfl))
  {
    status1 = cubic_minimizer(alphal, fl, dfl, alphat, ft, dft, 1, &alphac);
    status2 = quad_minimizer2(alphal, dfl, alphat, dft, &alphas);
  
    if(status1 == GSL_SUCCESS && status2 == GSL_SUCCESS)
    {
      if(fabs(alphac - alphat) < fabs(alphas - alphat))
        alphat_plus = alphac;
      else
        alphat_plus = alphas;
      
      if(alphat > alphal)
        alphat_plus = GSL_MIN_DBL(alphat + 0.66 * (alphau - alphat), alphat_plus);
      else
        alphat_plus = GSL_MAX_DBL(alphat + 0.66 * (alphau - alphat), alphat_plus);
    }
    else
      goto bisection2;
  }
  else
  {
    if(!state->alphau_evaluated)
    {
      gsl_blas_dcopy(setup->x, state->xt);
      gsl_blas_daxpy(state->alphau, setup->d, state->xt);
      GSL_MULTIMIN_FN_EVAL_F_DF(setup->fdf, state->xt, &state->phiu, state->gu);
      gsl_blas_ddot(state->gu, setup->d, &state->dphiu);
      state->alphau_evaluated = 1;
    }
  
    if(cubic_minimizer(alphat, ft, dft, alphau, fu, dfu, 1, &alphae) == GSL_SUCCESS)
      alphat_plus = alphae;
    else
      goto bisection2;
  }
  
  return alphat_plus;
  
  bisection1:
  //return 0.5 * (alphal + alphat);
  return alphal + 0.25 * (alphat - alphal);
  bisection2:
  //return 0.5 * (alphat + alphau);
  return alphat + 0.75 * (alphau - alphat);
}

static void bracket(double *alphal, 
                    double alphat, 
                    double *alphau, 
                    double fl, 
                    double ft, 
                    double dft, 
                    double *phil, 
                    double phit, 
                    double *phiu, 
                    double *dphil, 
                    double dphit, 
                    double *dphiu)
{
  if(ft > fl)                             /* Case 1. */
  {
    *alphau = alphat;
    *phiu = phit;
    *dphiu = dphit;
  }
  else
  {
    if(dft * (alphat - *alphal) < 0.0)    /* Case 2. */
    {
      *alphal = alphat;
      *phil = phit;
      *dphil = dphit;
    }
    else
    {
      *alphau = *alphal;                  /* Case 3. */
      *phiu = *phil;
      *dphiu = *dphil;
      *alphal = alphat;
      *phil = phit;
      *dphil = dphit;
    }
  }
}

static inline double psi(double phi, double alpha, double phi0, double dphi0)
{
  return phi - phi0 - mu * dphi0 * alpha;
}

static const lineminimizer_type morethuente_type = 
{
  "morethuente",
  sizeof(morethuente_state_t),
  &morethuente_alloc,
  &morethuente_minimize,
  &morethuente_free
};

const lineminimizer_type *lineminimizer_morethuente = &morethuente_type;
